/*********************************************************************
*
*   RobotMath.h - Robot math header
*
*   Copyright:
*       Software source code by Alex Morozov and Chris D. Locke is
*       licensed under a Creative Commons Attribution-Noncommercial-
*       Share Alike 3.0 United States License
*       (http://creativecommons.org/licenses/by-nc-sa/3.0/us/)
*
*********************************************************************/
#pragma once

/*----------------------------------------------------------
Literal constants
----------------------------------------------------------*/
#define EPSILON             ( 0.0001 )
#define MOTOR_SCALE         ( 0.2 )
#define PI                  ( 3.14159265 )
#define WHEEL_SPEED_EPSILON ( 0.1 / MOTOR_SCALE )

/*----------------------------------------------------------
Procedures
----------------------------------------------------------*/
void ComputeRotationAngle( double leftWheelSpeed,
                           double rightWheelSpeed,
                           double robotWidth,
                           double * angle,
                           double * distance );
double ConvertYawToTheta( double yaw );

void RotateAboutPoint( double * x, double * y, double xo, double yo, double angle );


